from machine import Pin,UART
import sensor, image, time
import seekfree, pyb
from seekfree import Servo
import math
# 初始化激光灯控制引脚，并置为高电平
#laser_light=Pin("C9", Pin.OUT)
#laser_light.value(1)
uart = UART(2, baudrate=115200)     # 初始化串口 波特率设置为115200 TX是B12 RX是B13
# 初始化TFT180屏幕
#lcd = seekfree.LCD180(3)
#创建舵机对象初始化
seekfree.init()
# 新建一个舵机对象
# 2：选择二通道，信号从C7输出（可设置范围是1-4，分别对应引脚C6 C7 C8 C9）
# 50：频率设置为50hz（可设置范围是50-300）
# 45：初始化角度设置为45度（可设置范围0-180）
servo_x = Servo(2,100,100)
servo_y = Servo(3,100,90)

class target_check(object):
    para1 = 0   #uin8
    para2 = 0   #uin8
    para3 = 0   #uin8
    para4 = 0   #uin16
    para5 = 0   #uin16
    para6 = 0   #uin16
    para7 = 0   #in32
    para8 = 0   #in32
    para9 = 0   #in32

pack=target_check()

class uart_buf_prase(object):
    uart_buf = []
    _data_len = 0
    _data_cnt = 0
    state = 0

HEADER=[0x11,0x13]
MODE=[0x01,0x02,0x03]

def package_blobs_data():
    #数据打包封装
    data=bytearray([HEADER[0],HEADER[1],MODE[0],0x00,
                   pack.para1,
                   pack.para2,
                   pack.para3,
                   pack.para4>>8,pack.para4&0xff,
                   pack.para5>>8,pack.para5&0xff,
                   pack.para6>>8,pack.para6&0xff,
                   pack.para7>>24&0xff,pack.para7>>16&0xff,
                   pack.para7>>8&0xff,pack.para7&0xff,
                   pack.para8>>24&0xff,pack.para8>>16&0xff,
                   pack.para8>>8&0xff,pack.para8&0xff,
                   pack.para9>>24&0xff,pack.para9>>16&0xff,
                   pack.para9>>8&0xff,pack.para9&0xff,
                   0x00])
    #数据包的长度
    data_len=len(data)
    data[3]=data_len-5#有效数据的长度
    #和校验
    sum=0
    for i in range(0,data_len-1):
        sum=sum+data[i]
    data[data_len-1]=sum%256
    #返回打包好的数据
    return data


class uart_buf_prase(object):
    uart_buf = []
    _data_len = 0
    _data_cnt = 0
    state = 0
R=uart_buf_prase()
#__________________________________________________________________
work_mode = 0
#串口数据解析
def Receive_Anl(data_buf,num):
    global work_mode,stop_flag
    #和校验
    sum = 0
    i = 0
    while i<(num-1):
        sum = sum + data_buf[i]
        i = i + 1
    sum = sum%256 #求余
    if sum != data_buf[num-1]:
        return
    #和校验通过
    if data_buf[2]==0xA0:
        #设置模块工作模式
        work_mode = data_buf[4]
        stop_flag = work_mode
        #print(ctr.work_mode)
        #print("Set work mode success!")

#__________________________________________________________________
def uart_data_prase(buf):
    if R.state==0 and buf==0x11:#帧头1
        R.state=1
        R.uart_buf.append(buf)
    elif R.state==1 and buf==0x13:#帧头2
        R.state=2
        R.uart_buf.append(buf)
    elif R.state==2 and buf<0xFF:#功能字
        R.state=3
        R.uart_buf.append(buf)
    elif R.state==3 and buf<50:#数据长度小于50
        R.state=4
        R._data_len=buf  #有效数据长度
        R._data_cnt=buf+5#总数据长度
        R.uart_buf.append(buf)
    elif R.state==4 and R._data_len>0:#存储对应长度数据
        R._data_len=R._data_len-1
        R.uart_buf.append(buf)
        if R._data_len==0:
            R.state=5
    elif R.state==5:
        R.uart_buf.append(buf)
        R.state=0
        Receive_Anl(R.uart_buf,R.uart_buf[3]+5)
#        print(R.uart_buf)
        R.uart_buf=[]#清空缓冲区，准备下次接收数据
    else:
        R.state=0
        R.uart_buf=[]#清空缓冲区，准备下次接收数据

#__________________________________________________________________


def uart_data_read():
    buf_len=uart.any()
    for i in range(0,buf_len):
        uart_data_prase(uart.readchar())
#__________________________________________________________________


# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565) # 设置图像色彩格式为RGB565格式
sensor.set_framesize(sensor.QVGA)  # 设置图像大小为160*120
sensor.set_auto_whitebal(False)      # 设置自动白平衡
sensor.set_brightness(200)         # 设置亮度为3000  150
sensor.skip_frames(time = 20)       # 跳过帧


clock = time.clock()
#101 88 #102 67  #78.5 67  #77.5 87.6
x = 100.8
y = 88.0
x0 = 100.8
y0 = 88.0
x1 = 102
y1 = 66.5
x2 = 78.5
y2 = 67
x3 = 77.5
y3 = 87.6

core_x0 = 0.0
core_y0 = 0.0
core_x1 = 0.0
core_y1 = 0.0
core_x2 = 0.0
core_y2 = 0.0
core_x3 = 0.0
core_y3 = 0.0
core_x_now = 0.0
core_y_now = 0.0
#矩形中心坐标
rect_x = 0
rect_y = 0
core_x_cut = 0.0
core_y_cut = 0.0
#路劲切分值
x_cut = 0.0
y_cut = 0.0
#切分数量
cut_num = 0
#当前目标误差
err_x = 0.0
err_y = 0.0
#总目标误差
err_x_mubiao = 0.0
err_y_mubiao = 0.0
err_x_cut = 0.0
err_y_cut = 0.0
out_x = 0.0
out_y = 0.0

qiehuan_x = 0.0
qiehuan_y = 0.0
#路劲总目标
core_x_mubiao = 0.0
core_y_mubiao = 0.0

core_flag = 10

lu_1 = 2
lu_2 = 2

lou_x = 0.0
lou_y =0.0
#切分累加值
lujing_x = 0.0
lujing_y = 0.0

flag = 0

stop_flag = 0
end_flag = 0

last_val_x = [0,0,0,0,0,0,0,0,0,0]#历史数值保存列表
def val_save_check_x(reslut):
    global last_val_x
    sum = 0
    last_val_x.pop(0)#弹出最后数据
    last_val_x.append(reslut)#更新最新的数据
    for a in range(0,10):
        sum = sum + last_val_x[a]
    return sum/10

last_val_y = [0,0,0,0,0,0,0,0,0,0]#历史数值保存列表
def val_save_check_y(reslut):
    global last_val_y
    sum = 0
    last_val_y.pop(0)#弹出最后数据
    last_val_y.append(reslut)#更新最新的数据
    for a in range(0,10):
        sum = sum + last_val_y[a]
    return sum/10

while(True):
    clock.tick()
    img = sensor.snapshot()#.lens_corr(strength = 1.5, zoom = 1.0)


# -----矩形框部分-----
    if flag == 0:
        for r in img.find_rects(threshold = 20000,roi = (73,23,184,191)):#(43,21,85,82)
            # 判断矩形边长是否符合要求
            if r.w() > 20 and r.h() > 20:
                # 在屏幕上框出矩形
                #img.draw_rectangle(r.rect(), color = (255, 0, 0), scale = 4)
                rect_x = r.x() + int(r.w()/2)
                rect_y = r.y() + int(r.h()/2)
                # 获取矩形角点位置
                corner = r.corners()
                a = corner[0][0]+int((rect_x-corner[0][0])/12)
                b = corner[0][1]+int((rect_y-corner[0][1])/12)
                # 在屏幕上圈出矩形角点
                img.draw_circle(a, b, 1, color = (0, 255, 255), thickness = 2, fill = False)
                core_x0 = a
                core_y0 = b
                a = corner[1][0]+int((rect_x-corner[1][0])/12)
                b = corner[1][1]+int((rect_y-corner[1][1])/12)
                img.draw_circle(a, b, 1, color = (0, 255, 255), thickness = 2, fill = False)
                core_x1 = a
                core_y1 = b
                a = corner[2][0]+int((rect_x-corner[2][0])/12)
                b = corner[2][1]+int((rect_y-corner[2][1])/12)
                img.draw_circle(a, b, 1, color = (0, 255, 255), thickness = 2, fill = False)
                core_x2  = a
                core_y2 = b
                a = corner[3][0]+int((rect_x-corner[3][0])/12)
                b = corner[3][1]+int((rect_y-corner[3][1])/12)
                img.draw_circle(a, b, 1, color = (0, 255, 255), thickness = 2, fill = False)
                core_x3 = a
                core_y3 = b
                flag = 1
    if flag == 1:
        if stop_flag == 0 :
            if core_flag == 0:
                core_x_mubiao = core_x0
                core_y_mubiao = core_y0
                if cut_num == 0:
                    #x_cut = (core_x3 - core_x0)/50
                   #y_cut = (core_y3 - core_y0)/50
                    #core_x_now = core_x3
                    #core_y_now = core_y3
                    lujing_x = 0
                    lujing_y = 0
                if cut_num < 100:
                    x_cut = (core_x3 - core_x0)/100
                    y_cut = (core_y3 - core_y0)/100
                    lujing_x = lujing_x + x_cut
                    lujing_y = lujing_y + y_cut
                    core_x_now = core_x3 - lujing_x
                    core_y_now = core_y3 - lujing_y
                    #core_x_now = core_x_now - x_cut
                   # core_y_now = core_y_now - y_cut
                    cut_num = cut_num + 1
                else:
                    pass
                    #core_flag = 1
                    #cut_num = 0
                    #end_flag = 1
                #core_x_cut = (core_x3 - core_x0)
                #core_y_cut = (core_y3 - core_y0)
            elif core_flag == 1:
                core_x_mubiao = core_x1
                core_y_mubiao = core_y1
                if cut_num == 0:
                    #x_cut = (core_x0 - core_x1)/50
                    #y_cut = (core_y0 - core_y1)/50
                    #core_x_now = core_x0
                    #core_y_now =  core_y0
                    lujing_x = 0
                    lujing_y = 0
                if cut_num < 100:
                    x_cut = (core_x0 - core_x1)/100
                    y_cut = (core_y0 - core_y1)/100
                    lujing_x = lujing_x + x_cut
                    lujing_y = lujing_y + y_cut
                    core_x_now = core_x0 - lujing_x
                    core_y_now = core_y0 - lujing_y
                    cut_num = cut_num + 1
                else:
                    pass
                    #core_flag = 2
                    #cut_num = 0
                #core_x_cut = (core_x0 - core_x1)
                #core_y_cut = (core_y0 - core_y1)
            elif core_flag == 2:
                #lu_2 = 2
                #lu_1 = 2.2
                core_x_mubiao = core_x2
                core_y_mubiao = core_y2
                if cut_num == 0:
                    #x_cut = (core_x1 - core_x2)/50
                    #y_cut = (core_y1 - core_y2)/50
                    #core_x_now =  core_x1
                    #core_y_now =  core_y1
                    lujing_x = 0
                    lujing_y = 0
                if cut_num < 100:
                    y_cut = (core_y1 - core_y2)/100
                    x_cut = (core_x1 - core_x2)/100
                    lujing_x = lujing_x + x_cut
                    lujing_y = lujing_y + y_cut
                    core_x_now = core_x1 - lujing_x
                    core_y_now = core_y1 - lujing_y
                    cut_num = cut_num + 1
                else :
                    pass
                    #core_flag = 3
                    #cut_num = 0
                #core_x_cut = (core_x1 - core_x2)
                #core_y_cut = (core_y1 - core_y2)
                #lu_1 = 0.94
            elif core_flag == 3:
                #lu_2 = 2
                #lu_1 = 2.2
                core_x_mubiao = core_x3
                core_y_mubiao = core_y3
                if cut_num == 0:
                    #x_cut = (core_x2 - core_x3)/50
                    #y_cut = (core_y2 - core_y3)/50
                    #core_x_now =  core_x2
                    #core_y_now =  core_y2
                    lujing_x = 0
                    lujing_y = 0
                if cut_num < 100:
                    x_cut = (core_x2 - core_x3)/100
                    y_cut = (core_y2 - core_y3)/100
                    lujing_x = lujing_x + x_cut
                    lujing_y = lujing_y + y_cut
                    core_x_now = core_x2 - lujing_x
                    core_y_now = core_y2 - lujing_y
                    cut_num = cut_num + 1
                else:
                    pass
                    #core_flag = 0
                    #cut_num = 0
                #lu_1 = 2
                #lu_2 = 0.93
                #core_x_cut = (core_x2 - core_x3)
                #core_y_cut = (core_y2 - core_y3)
            elif  core_flag == 10:
                cut_num = 100
                core_x_mubiao = core_x0
                core_y_mubiao = core_y0
                core_x_now = core_x0
                core_y_now = core_y0
    # 打印帧率
# -----跟踪激光部分-----
    # 设置激光颜色阈值
    red_td = [(0, 127, 26, 127, -128, 127)]
    # 根据阈值找到色块
    for b in img.find_blobs(red_td,pixels_threshold=1, area_threshold=1, merge=False,invert = 0,roi = (68,24,191,181)):
        # 在屏幕上画出色块
        img.draw_rectangle(b.rect(), color = (0, 255, 0), scale = 1, thickness = 1)
        err_x = (b.x()+b.w()/2) - core_x_now
        err_y = (b.y()+b.h()/2) - core_y_now
        err_x_mubiao = (b.x()+b.w()/2) - core_x_mubiao
        err_y_mubiao = (b.y()+b.h()/2) - core_y_mubiao
        if 1:
            pass
            #err_x_cut = (err_x)/(abs(err_x)+abs(err_y))
            #err_y_cut = (err_y)/(abs(err_x)+abs(err_y))
        if math.sqrt(err_x_mubiao*err_x_mubiao + err_y_mubiao*err_y_mubiao)<=2 and (cut_num >= 100):
            cut_num = 0
            if core_flag == 10:
                core_flag = 1
            else :
                core_flag = core_flag+1
            if core_flag == 4:
                core_flag = 0



        break

    # 显示到屏幕上，此部分会降低帧率
    #lcd.show_image(img, 320, 240, 0, 0, zoom=0)  #屏幕显示
    print(pack.para8,pack.para9,err_x,err_y,core_flag,cut_num,end_flag,work_mode)
    if 1:
        #out_x = err_x*0.025
        #out_x = pan_pid.get_pid(err_x,0.1)
        #out_y =  - err_y*0.025
        #out_y = tilt_pid.get_pid(-err_y,0.1)
        err_x_cut = (out_x/50)
        err_y_cut = (out_y/50)

    if end_flag == 1:
        err_x = 0
        err_y = 0

    pack.para8 = int(err_x*10)
    pack.para9 = int(err_y*10)


    #舵机输出
    uart.write(package_blobs_data())
    uart_data_read()
    #servo_x.angle(servo_x.angle()+out_x)
    #servo_y.angle(servo_y.angle()+ out_y)
    #pyb.mdelay(3)
    if out_y >100 :
        out_y = 100
    #servo_y.angle(out_y)
#任务1
    '''
    for a in range(0,101):
        y = y + (y1-y0)/100
        x = x + (x1-x0)/100
        servo_y.angle(y)#75,8   #99
        servo_x.angle(x)
        pyb.mdelay(30)
    for a in range(0,101):
        y = y + (y2-y1)/100
        x = x + (x2-x1)/100
        servo_y.angle(y)#75,8   #99
        servo_x.angle(x)
        pyb.mdelay(30)
    for a in range(0,101):
        y = y + (y3-y2)/100
        x = x + (x3-x2)/100
        servo_y.angle(y)#75,8   #99
        servo_x.angle(x)
        pyb.mdelay(30)
    for a in range(0,101):
        y = y + (y0-y3)/100
        x = x + (x0-x3)/100
        servo_y.angle(y)#75,8   #99
        servo_x.angle(x)
        pyb.mdelay(30)
    '''
    #101 88 #102 67  #78.5 67  #77.5 87.6



